/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "CScanMeasurementSet.h"


void printUsage(){
	fprintf(stderr,"\n-----------------------------------------------------------------\n");
	fprintf(stderr,"Opens MaCI/LnMlib logfiles and puts an estimated (by MRPT) pose traj\n");
	fprintf(stderr,"Usage: ./OpenLogAddEstimteAndSave [method] ...\n");
	fprintf(stderr,"Method=0: ./OpenLogAddEstimteAndSave 0 [in_position] [in_ranging] [in_corrected]\n");
	fprintf(stderr,"Method=1: ./OpenLogAddEstimteAndSave 1 [in_Lnmlib] [in_corrected]\n");
	fprintf(stderr,"\nThe output is saved to 'dataset.txt'\n");
	fprintf(stderr,"-----------------------------------------------------------------\n\n");
}

vector<Grid::pose> poses; 

void readEstimatedTrajectory(char *corrected_file_name){
	FILE *f = fopen(corrected_file_name,"rt");
	if(f==NULL){
		fprintf(stderr,"File %s Not found\n",corrected_file_name);
		exit(1);
	}
	char in_line[16096];
	fgets (in_line, 16096, f );
	int cnt=0;
	int i=0;
	char c=0;
	while( (c!='\n')){
		if(in_line[i]==' ') cnt++;
		else if(in_line[i]=='\n') break;
		i++;
	}
	fprintf(stderr,"There is %d values in the logfile!\n",cnt);
	fclose(f);
	f = fopen(corrected_file_name,"rt");
	
	
	int num=0;
	float z,r,pit;
	while(!feof(f)){
		Grid::pose p;
		
		if(cnt==2){
			fscanf(f,"%f %f %f ",&p.x,&p.y,&p.a); ///< Pose header
		}else if(cnt==3){
			fscanf(f,"%d %f %f %f ",&num, &p.x,&p.y,&p.a); ///< Pose header
		}else if(cnt==6){
			fscanf(f,"%d %f %f %f %f %f %f ",&num, &p.x, &p.y, &z, &p.a, &r, &pit); ///< Pose header
		}else{
			fprintf(stderr,"Mutha Fucha log file error!!!\n");
			exit(1);
		}
			
		poses.push_back(p);
	}
	fclose(f);
}


int main(int argc, char *argv[])
{
	
	if(argc<2){
		printUsage();
		exit(1);
	}
	
	int method = atoi(argv[1]);
	/// Open MaCI/LnMlib logfiles
	char *position_file_name=NULL;
	char *ranging_file_name=NULL;
	char *measurement_file_name=NULL;
	char *corrected_file_name=NULL;
	
	switch(method){
		case 0:
			position_file_name = argv[2];
			ranging_file_name = argv[3];
			corrected_file_name = argv[4];
			
			fprintf(stderr,"opening Position='%s', Ranging='%s' and Corrected='%s'\n", position_file_name, ranging_file_name, 
							corrected_file_name);
			break;
		case 1:
			measurement_file_name = argv[2];
			corrected_file_name = argv[3];
			
			fprintf(stderr,"opening LnMfile='%s' and Corrected='%s'\n", measurement_file_name, ranging_file_name, 
							corrected_file_name);
			
			break;
		default:
			printUsage();
			exit(1);
			break;
	}

	CScanMeasurementSet *dset;
	
	if(position_file_name != NULL){
		dset = new CScanMeasurementSet(position_file_name,ranging_file_name);
	}else{
		dset = new CScanMeasurementSet(measurement_file_name);
	}
	fprintf(stderr,"DATASET size = %d Nummeas=%d\n", dset->size(),dset->angles.size());
	
	readEstimatedTrajectory(corrected_file_name);
	
	if(poses.size() == dset->ranges.size()){
		dset->poses = poses;
		
		dset->saveLnMFormat("lnm_dataset.txt");
		dset->saveMaCIPositionFILE("MaCI_dataset.txt",
															 "LOG",
															 "Matched");
	}else{
		fprintf(stderr,"The datasets do not match POSES = %d Ranges=%d!!\n",poses.size(),dset->ranges.size());
	}
	
	
	return 0;
}